﻿using System;
using System.Net;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Documents;
using System.Windows.Ink;
using System.Windows.Input;
using System.Windows.Media;
using System.Windows.Media.Animation;
using System.Windows.Shapes;


namespace Skybot.Arcgis.Client.TileLayerExpand
{
    using ESRI.ArcGIS.Client;
    using System.Runtime.CompilerServices;
    using ESRI.ArcGIS.Client.Geometry;
    using System.Text;
    using System.Globalization;
    using System.Collections.ObjectModel;
    using System.Linq;
    using System.Diagnostics;
   
    /// <summary>
    ///两个坐标系转换一般需要平移，旋转，缩放共七参数。 Y=（1＋k)*M(x,y,z)*X+dX; X,Y为3*1矩阵,M(x,y,z)为3*3的旋转矩阵. public class CoordTrans7Param { public double[,] values=new double[7,1]; //{{dx},{dy},{dz},{rx},{ry},{rz},{k}}; //public double　　 两个坐标系转换一般需要平移，旋转，缩放共七参数。 
    ///Y=（1＋k)*M(x,y,z)*X+dX; 
    ///X,Y为3*1矩阵,M(x,y,z)为3*3的旋转矩阵
    /// </summary>
    public class CoordTrans7Param
    {
        public double[,] values = new double[7, 1];
        //{{dx},{dy},{dz},{rx},{ry},{rz},{k}}; 
        //public double dx,dy,dz,rx,ry,rz,k; 
        public void Set4Param(double dx, double dy, double dz, double k)
        {
            this.dx = dx;
            this.dy = dy;
            this.dz = dz;
            this.k = k;
            this.rx = this.ry = this.rz = 0;
        }
        public void SetRotationParamRad(double rx, double ry, double rz)
        {
            this.rx = rx;
            this.ry = ry;
            this.rz = rz;
        }
        public void SetRotationParamMM(double rx, double ry, double rz)
        {
            SetRotationParamRad(rx * Math.PI / 648000, ry * Math.PI / 648000, rz * Math.PI / 648000);
        }
        private double[,] GetMx()
        {
            double[,] Mx = new double[,] 
{{1,0,0}, 
{0,Math.Cos(rx),Math.Sin(rx)}, 
{0,-Math.Sin(rx),Math.Cos(rx)}};
            return Mx;
        }
        private double[,] GetMy()
        {
            double[,] My = new double[,] 
{{Math.Cos(ry),0,-Math.Sin(ry)}, 
{0,1,0}, 
{Math.Sin(ry),0,Math.Cos(ry)}};
            return My;
        }
        private double[,] GetMz()
        {
            double[,] Mz = new double[,] 
{{Math.Cos(rz),Math.Sin(rz),0}, 
{-Math.Sin(rz),Math.Cos(rz),0}, 
{0,0,1}};
            return Mz;
        }

        private double[,] GetM() //M=Mx*My*Mz? or M=Mz*My*Mx? 
        {
            double[,] M = new double[3, 3];

            MatrixTool.Multi(GetMz(), GetMy(), ref M);
            MatrixTool.Multi(M, GetMx(), ref M);
            return M;
        }
        private double[,] GetMdx()
        {
            double[,] mt = {{ 0, 0, 0 }, 
{ 0, -Math.Sin(rx), Math.Cos(rx) }, 
{ 0, -Math.Cos(rx), -Math.Sin(rx) }};

            double[,] m = new double[3, 3];

            MatrixTool.Multi(GetMz(), GetMy(), ref m);
            MatrixTool.Multi(m, mt, ref m);
            return m;
        }
        private double[,] GetMdy()
        {
            double[,] mt = {{ -Math.Sin(ry), 0, -Math.Cos(ry) }, 
{ 0, 0, 0 }, 
{ Math.Cos(ry), 0, -Math.Sin(ry) }};

            double[,] m = new double[3, 3];

            MatrixTool.Multi(GetMz(), mt, ref m);
            MatrixTool.Multi(m, GetMx(), ref m);
            return m;
        }

        private double[,] GetMdz()
        {
            double[,] mt = {{ -Math.Sin(rz), Math.Cos(rz), 0 }, 
{ -Math.Cos(rz), -Math.Sin(rz), 0 }, 
{ 0, 0, 0 }};

            double[,] m = new double[3, 3];

            MatrixTool.Multi(mt, GetMy(), ref m);
            MatrixTool.Multi(m, GetMx(), ref m);
            return m;
        }
        private double[,] specialMulti(double[,] m, double[,] X)
        {
            int rowNumM = m.GetLength(0);
            int colNumM = m.GetLength(1);
            int rowNumX = X.GetLength(0);
            int colNumX = X.GetLength(1);
            int lines = rowNumX / colNumM;
            double[,] mt = MatrixTool.Init(rowNumM, colNumX);
            double[,] subX = MatrixTool.Init(colNumM, colNumX);
            double[,] res = MatrixTool.Init(rowNumM * lines, colNumX);

            for (int i = 0; i < lines; i++)
            {
                MatrixTool.CopySub(X, i * colNumM, 0, colNumM, colNumX, ref subX, 0, 0);
                MatrixTool.Multi(m, subX, ref mt);
                MatrixTool.CopySub(mt, 0, 0, rowNumM, colNumX, ref res, i * rowNumM, 0);
            }
            return res;
        }
        private double[,] specialSub(double[,] m, double[,] X)
        {
            int rowNumM = m.GetLength(0);
            int colNumM = m.GetLength(1);
            int rowNumX = X.GetLength(0);
            int colNumX = X.GetLength(1);
            int lines = rowNumX / rowNumM;
            double[,] subX = MatrixTool.Init(rowNumM, colNumX);
            double[,] res = MatrixTool.Init(rowNumX, colNumX);

            for (int i = 0; i < rowNumX; i += rowNumM)
            {
                MatrixTool.CopySub(X, i, 0, rowNumM, colNumX, ref subX, 0, 0);
                MatrixTool.Sub(m, subX, ref subX);
                MatrixTool.CopySub(subX, 0, 0, rowNumM, colNumX, ref res, i, 0);
            }
            return res;
        }

        private double[,] GetF(double[,] X, double[,] Y)
        {
            double[,] f0;
            double[,] qx = MatrixTool.Init(X.GetLength(0), 1);
            double[,] K = { { -dx }, { -dy }, { -dz } };
            double[,] S = { { 1 + k } };

            MatrixTool.Multi(X, S, ref qx);
            double[,] M = GetM();
            qx = specialMulti(M, qx);
            MatrixTool.Sub(qx, Y, ref qx);
            f0 = specialSub(K, qx);
            return f0;
        }
        private double[,] GetB(double[,] X)
        {
            int rowNum = X.GetLength(0);
            double[,] B = MatrixTool.Init(rowNum, 7);
            double[,] M = GetM();
            double[,] Mdx = GetMdx();
            double[,] Mdy = GetMdy();
            double[,] Mdz = GetMdz();
            double[,] mi = MatrixTool.Ident(3);
            double[,] MX, MY, MZ, MK;

            MK = specialMulti(M, X);
            MX = specialMulti(Mdx, X);
            MY = specialMulti(Mdy, X);
            MZ = specialMulti(Mdz, X);

            for (int i = 0; i < rowNum; i += 3)
                MatrixTool.CopySub(mi, 0, 0, 3, 3, ref B, i, 0);
            MatrixTool.CopySub(MX, 0, 0, rowNum, 1, ref B, 0, 3);
            MatrixTool.CopySub(MY, 0, 0, rowNum, 1, ref B, 0, 4);
            MatrixTool.CopySub(MZ, 0, 0, rowNum, 1, ref B, 0, 5);
            MatrixTool.CopySub(MK, 0, 0, rowNum, 1, ref B, 0, 6);
            return B;
        }
        private double[,] GetA()
        {
            double[,] M = GetM();
            double[,] I2 = MatrixTool.Ident(3);
            double[,] A = MatrixTool.Init(3, 6);

            MatrixTool.MutliConst(ref I2, -1);
            MatrixTool.MutliConst(ref M, (1 + k));
            MatrixTool.CopySub(M, 0, 0, 3, 3, ref A, 0, 0);
            MatrixTool.CopySub(I2, 0, 0, 3, 3, ref A, 0, 3);
            return A;
        }
        private double[,] GetV(double[,] X, double[,] Y, CoordTrans7Param dpp)
        {
            int rowNum = X.GetLength(0);

            double[,] B, F, A, B2, B3, F2, V;
            double[,] AT = MatrixTool.Init(6, 3);

            A = GetA();
            MatrixTool.AT(A, ref AT);
            MatrixTool.MutliConst(ref AT, 1 / (1 + (1 + k) * (1 + k)));

            F = GetF(X, Y);
            B = GetB(X);
            B2 = MatrixTool.Init(3, 7);
            B3 = MatrixTool.Init(3, 1);
            F2 = MatrixTool.Init(rowNum, 1);
            for (int i = 0; i < rowNum / 3; i++)
            {
                MatrixTool.CopySub(B, i * 3, 0, 3, 7, ref B2, 0, 0);
                MatrixTool.Multi(B2, dpp.values, ref B3);
                MatrixTool.CopySub(B3, 0, 0, 3, 1, ref F2, i * 3, 0);
            }
            MatrixTool.Sub(F, F2, ref F2);
            V = specialMulti(AT, F2);
            return V;
        }
        public double CalculateTrans7Param(double[,] X, double[,] Y)
        {
            int PtNum = X.GetLength(0) / 3;
            double[,] B;
            double[,] F;
            double[,] BT = MatrixTool.Init(7, 3 * PtNum);
            double[,] BTB = MatrixTool.Init(7, 7);
            double[,] BTF = MatrixTool.Init(7, 1);


            //init pararm 
            CoordTrans7Param dpp = new CoordTrans7Param();
            Set4Param(0, 0, 0, 0);
            this.SetRotationParamMM(0, 0, 0);
            //debug 
            //this.TransCoord(X[0,0],X[1,0],X[2,0],out x2,out y2,out z2); 
            int round = 0;
            while (round++ < 20)
            {
                F = GetF(X, Y);
                B = GetB(X);
                MatrixTool.AT(B, ref BT);

                MatrixTool.Multi(BT, B, ref BTB);
                MatrixTool.Inv(BTB);
                MatrixTool.Multi(BT, F, ref BTF);
                MatrixTool.Multi(BTB, BTF, ref dpp.values);
                if (dpp.isSmall())
                    break;
                else
                    MatrixTool.Add(this.values, dpp.values, ref this.values);
            }
            //this.TransCoord(X[0,0],X[1,0],X[2,0],out x2,out y2,out z2); 
            double[,] V = GetV(X, Y, dpp);

            double vMax = -1;
            for (int i = 0; i < V.GetLength(0); i++)
            {
                if (Math.Abs(V[i, 0]) > vMax)
                    vMax = Math.Abs(V[i, 0]);
            }

            return vMax;
        }
        private bool isSmall()
        {
            double s = 0;
            for (int i = 0; i < 7; i++)
                s += Math.Abs(values[i, 0]);
            if (s < 0.0000001)
                return true;
            else
                return false;
        }
        public void TransCoord(double x1, double y1, double z1, out double x2, out double y2, out double z2)
        {

            double[,] Xi = { { x1 }, { y1 }, { z1 } };
            double[,] DX = { { dx }, { dy }, { dz } };
            double[,] tY = new double[3, 1];
            double[,] K = { { 1 + k } };

            double[,] M = GetM();
            MatrixTool.Multi(Xi, K, ref tY);
            MatrixTool.Multi(M, tY, ref tY);
            MatrixTool.Add(tY, DX, ref tY);
            x2 = tY[0, 0];
            y2 = tY[1, 0];
            z2 = tY[2, 0];
        }

        public double dx
        {
            get
            {
                return values[0, 0];
            }
            set
            {
                values[0, 0] = value;
            }
        }
        public double dy
        {
            get
            {
                return values[1, 0];
            }
            set
            {
                values[1, 0] = value;
            }
        }
        public double dz
        {
            get
            {
                return values[2, 0];
            }
            set
            {
                values[2, 0] = value;
            }
        }
        public double rx
        {
            get
            {
                return values[3, 0];
            }
            set
            {
                values[3, 0] = value;
            }
        }
        public double ry
        {
            get
            {
                return values[4, 0];
            }
            set
            {
                values[4, 0] = value;
            }
        }
        public double rz
        {
            get
            {
                return values[5, 0];
            }
            set
            {
                values[5, 0] = value;
            }
        }
        public double k
        {
            get
            {
                return values[6, 0];
            }
            set
            {
                values[6, 0] = value;
            }
        }
    }


    public class MatrixTool
    {
        public static double[,] Init(int m, int n)
        {
            double[,] M = new double[m, n];
            for (int i = 0; i < m; i++)
                for (int j = 0; j < n; j++)
                    M[i, j] = 0;
            return M;
        }

        public static double[,] Ident(int rank)
        {
            double[,] m = new double[rank, rank];
            for (int i = 0; i < rank; i++)
                for (int j = 0; j < rank; j++)
                {
                    if (i == j)
                        m[i, j] = 1.0;
                    else
                        m[i, j] = 0;
                }
            return m;
        }
        public static void MutliConst(ref double[,] m1, double c)
        {
            for (int i = 0; i < m1.GetLength(0); i++)
                for (int j = 0; j < m1.GetLength(1); j++)
                {
                    m1[i, j] *= c;
                }
        }
        public static void Copy(double[,] m1, ref double[,] m2)
        {
            for (int i = 0; i < m1.GetLength(0); i++)
                for (int j = 0; j < m1.GetLength(1); j++)
                {
                    m2[i, j] = m1[i, j];
                }
        }
        public static void CopySub(double[,] m1, int rowStart, int colStart, int rowNum, int colNum, ref double[,] m2, int rowStart2, int colStart2)
        {
            for (int i1 = rowStart, i2 = rowStart2; i1 < rowStart + rowNum; i1++, i2++)
                for (int j1 = colStart, j2 = colStart2; j1 < colStart + colNum; j1++, j2++)
                {
                    m2[i2, j2] = m1[i1, j1];
                }
        }
        public static void Multi(double[,] m1, double[,] m2, ref double[,] mout)
        {
            int m1x, m1y, m2x, m2y, moutx, mouty;

            if (m1.Rank != 2 || m2.Rank != 2 || mout.Rank != 2)
                throw new Exception("Multi 输入错误!");
            m1x = m1.GetLength(0);
            m1y = m1.GetLength(1);

            m2x = m2.GetLength(0);
            m2y = m2.GetLength(1);

            moutx = mout.GetLength(0);
            mouty = mout.GetLength(1);


            if (m1y != m2x || m1x != moutx || m2y != mouty)
                throw new Exception("Multi 输入错误!");

            double[,] mtemp = new double[moutx, mouty];
            for (int i = 0; i < m1x; i++)
                for (int j = 0; j < m2y; j++)
                {
                    mtemp[i, j] = 0;
                    for (int k = 0; k < m1y; k++)
                        mtemp[i, j] += m1[i, k] * m2[k, j];
                }
            Copy(mtemp, ref mout);
        }
        public static void Add(double[,] m1, double[,] m2, ref double[,] mout)
        {
            int m1x, m1y, m2x, m2y;

            if (m1.Rank != 2 || m2.Rank != 2 || mout.Rank != 2)
                throw new Exception("Matrix.Add 输入错误!");
            m1x = m1.GetLength(0);
            m1y = m1.GetLength(1);

            m2x = m2.GetLength(0);
            m2y = m2.GetLength(1);

            if (m1x != m2x || m1y != m2y)
                throw new Exception("Matrix.Add 输入错误!");
            if (mout.GetLength(0) != m1x || mout.GetLength(1) != m2y)
                throw new Exception("Matrix.Add 输入错误!");
            //mout=new double[m1x,m2y]; 
            for (int i = 0; i < m1x; i++)
                for (int j = 0; j < m2y; j++)
                    mout[i, j] = m1[i, j] + m2[i, j];
        }
        public static void Sub(double[,] m1, double[,] m2, ref double[,] mout)
        {
            int m1x, m1y, m2x, m2y;

            if (m1.Rank != 2 || m2.Rank != 2 || mout.Rank != 2)
                throw new Exception("Matrix.Sub 输入错误!");
            m1x = m1.GetLength(0);
            m1y = m1.GetLength(1);

            m2x = m2.GetLength(0);
            m2y = m2.GetLength(1);

            if (m1x != m2x || m1y != m2y)
                throw new Exception("Matrix.Sub 输入错误!");
            if (mout.GetLength(0) != m1x || mout.GetLength(1) != m2y)
                throw new Exception("Matrix.Sub 输入错误!");
            //mout=new double[m1x,m2y]; 
            for (int i = 0; i < m1x; i++)
                for (int j = 0; j < m2y; j++)
                    mout[i, j] = m1[i, j] - m2[i, j];
        }
        public static void AT(double[,] m1, ref double[,] mout)
        {
            for (int i = 0; i < m1.GetLength(0); i++)
                for (int j = 0; j < m1.GetLength(1); j++)
                {
                    mout[j, i] = m1[i, j];
                }
        }
        public static void ATBA(double[,] m1, double[,] m2, ref double[,] mout)
        {
            int M, N;
            M = m1.GetLength(0);
            N = m1.GetLength(1);
            if (mout.GetLength(0) != mout.GetLength(1) || mout.GetLength(0) != M)
                throw new Exception("ATBA 输入错误!");
            for (int i = 0; i < N; i++)
                for (int j = 0; j < N; j++)
                {
                    mout[i, j] = 0;
                    for (int r = 0; r < M; r++)
                        for (int k = 0; k < M; k++)
                            mout[i, j] = mout[i, j] + m1[k, i] * m2[k, r] * m1[r, j];

                }
        }
        public static void ABAT(double[,] m1, double[,] m2, ref double[,] mout)
        {
            int M, N;
            M = m1.GetLength(0);
            N = m1.GetLength(1);
            if (mout.GetLength(0) != mout.GetLength(1) || mout.GetLength(0) != M)
                throw new Exception("ATBA 输入错误!");
            for (int i = 0; i < M; i++)
                for (int j = 0; j < M; j++)
                {
                    mout[i, j] = 0;
                    for (int r = 0; r < N; r++)
                        for (int k = 0; k < N; k++)
                            mout[i, j] = mout[i, j] + m1[i, k] * m2[k, r] * m1[j, r];
                }
        }
        public static void Inv(double[,] c)
        {


            double temp = 0;
            int i, j, k, N = c.GetLength(0);

            //debug 
            for (i = 1; i < N; i++)
                for (j = 0; j < i; j++)
                    c[i, j] = 0;
            for (i = 0; i < N; i++)
            {
                for (j = i; j < N; j++)
                {
                    temp = c[i, j];
                    for (k = 0; k < i; k++)
                        temp = temp - c[k, i] * c[k, j] / c[k, k];
                    if (j == i)
                        c[i, j] = 1 / temp;
                    else
                        c[i, j] = temp * c[i, i];
                }
            }

            for (i = 0; i < N - 1; i++)
            {
                for (j = i + 1; j < N; j++)
                {
                    temp = -c[i, j];
                    for (k = i + 1; k < j; k++)
                    {
                        temp = temp - c[i, k] * c[k, j];
                    }
                    c[i, j] = temp;
                }
            }

            for (i = 0; i < N - 1; i++)
            {
                for (j = i; j < N; j++)
                {
                    if (j == i)
                        temp = c[i, j];
                    else
                        temp = c[i, j] * c[j, j];
                    for (k = j + 1; k < N; k++)
                        temp = temp + c[i, k] * c[j, k] * c[k, k];
                    c[i, j] = temp;
                }
            }
            for (i = 1; i < N; i++)
                for (j = 0; j < i; j++)
                    c[i, j] = c[j, i];
        }
        public static void DEBUG_DUMP(double[,] m1)
        {
            int M, N;
            string buf;

            M = m1.GetLength(0);
            N = m1.GetLength(1);
            Debug.WriteLine("****************debug matrix****************");
            for (int i = 0; i < M; i++)
            {
                buf = "";
                for (int j = 0; j < N; j++)
                {
                    buf += m1[i, j].ToString("0.000000");
                    buf += ",";
                }
                Debug.WriteLine(buf);
            }
            Debug.WriteLine("**************** end ****************");
        }
    }



}
